/**
 * @file SolverAmb.cpp
 * Class to compute the PPP Solution.
 *
 * modification
 *
 * 2020/01/22
 * add lambda and partial-ambiguity fixing method.
 *
 * 2020/01/25
 * type-related bias is computed and stored.
 * 
 * 2020/01/26
 * choose the robust receiver bias datum using median method
 * instead of previous ambiguity-datum
 *
 * 2020/02/06
 * an robust iterative receiver bias selection method is introduced.
 *
 * 2020/02/07 
 * update the interface of all functions 
 * 
 * @author shjzhang 2019/07/01
 * 
 */

#include "Edge.hpp"
#include "SolverAmb.hpp"
#include "MatrixFunctors.hpp"
#include "Matrix.hpp"
#include "Kruskal.hpp"
#include "Counter.hpp"
#include "DataStructures.hpp"
#include "Stats.hpp"

#define debug 1

namespace gpstk
{

      // Returns a string identifying this object.
    std::string SolverAmb::getClassName() const
    { return "SolverAmb"; }

    void SolverAmb::init()
    {
        // do nothing until now
    };

    void SolverAmb::Solve(VariableEstDataMap& ambEstData)
    {

        VariableDataMap floatAmbData;
        CovarianceDataMap floatAmbCovData;
        ambSet.clear();
        for(auto amb: ambEstData)
        {
            floatAmbData[amb.first] = amb.second.value;
            floatAmbCovData[amb.first][amb.first] = amb.second.variance;
            ambSet.insert(amb.first);
        }

        // Nfloat = B - br - bs;
        VariableDataMap floatIntAmbData;
        computeFloatIntAmbs(floatAmbData, floatIntAmbData);

        // Nfixed = AR(Nfloat);
        VariableDataMap fixedIntAmbData;
        fixWithRound(floatIntAmbData, floatAmbCovData, fixedIntAmbData);

        // BFixed = br + bs + Nfixed;
        VariableDataMap fixedFloatAmbData;
        computeFixedFloatAmbs(fixedIntAmbData, fixedFloatAmbData);

        currFloatIntAmbData = floatIntAmbData;
        currFixedIntAmbData = fixedIntAmbData;
        currFixedFloatAmbData = fixedFloatAmbData;

    }

    void SolverAmb::Solve(VariableDataMap& ambData, 
                          CovarianceDataMap& ambCovData, 
                          int method)
    {

        ambSet.clear();
        for( auto amb: ambData)
        {
            ambSet.insert(amb.first);
        }

        VariableDataMap floatAmbData;
        CovarianceDataMap floatAmbCovData;

        floatAmbData = ambData;
        floatAmbCovData = ambCovData;

        // remove receiver bias and compute float integer ambiguities
        VariableDataMap floatIntAmbData;
        computeFloatIntAmbs(floatAmbData, floatIntAmbData);

        cout << "floatIntAmbData:" << endl;
        for(auto amb: floatIntAmbData)
        {
            cout << amb.first << ": " << amb.second <<endl;
        }


        VariableDataMap fixedIntAmbData;
        if(method==1)
        {
            fixWithRound(floatIntAmbData, floatAmbCovData, fixedIntAmbData);
        }
        else if(method==2)
        {
            cout << "fixing with PAR" << endl;
            fixWithPAR(floatIntAmbData, floatAmbCovData, fixedIntAmbData);
        }

        cout << "fixedIntAmbData:" << endl;
        for(auto amb: fixedIntAmbData)
        {
            cout << amb.first << ": " << amb.second <<endl;
        }

        // compute fixed float ambs
        VariableDataMap fixedFloatAmbData;
        computeFixedFloatAmbs(fixedIntAmbData, fixedFloatAmbData);

        cout << "fixedFloatAmbData:" << endl;
        for(auto amb: fixedFloatAmbData)
        {
            cout << amb.first << ": " << amb.second <<endl;
        }

        currFloatIntAmbData = floatIntAmbData;
        currFixedIntAmbData = fixedIntAmbData;
        currFixedFloatAmbData = fixedFloatAmbData;

        cout << currFloatIntAmbData.size() << endl;
        cout << currFixedIntAmbData.size() << endl;
        cout << currFixedFloatAmbData.size() << endl;

        return;
    }


    void SolverAmb::computeFloatIntAmbs(VariableDataMap& floatAmbData,
                                        VariableDataMap& floatIntAmbData)
    {
        // 
        // Step 1: remove satellite biases from all ambiguities
        //
        VariableDataMap floatAmbDataCorrected;
        for(auto amb: floatAmbData )
        {
            SatID  sat( amb.first.getSatellite() );
            TypeID type( amb.first.getType() );
            try
            {
                double bias = satTypeBiasData.getValue(sat, type);
                floatAmbDataCorrected[amb.first] = amb.second - bias;
            }
            catch(Exception& e)
            {
                cerr << "don't find satellite bias in satTypeBiasData!" << endl;;
                exit(-1);
            }
        }

        // step2: classify ambiguities
        systemTypeVarsMap systemTypeVars;
        for(auto amb: ambSet)
        {
            SatID sat = amb.getSatellite();
            SatelliteSystem::Systems system = sat.getSatelliteSystem();
            TypeID type = amb.getType();
            VariableSet varSet = systemTypeVars[system][type];
            varSet.insert(amb);
            systemTypeVars[system][type] = varSet;
        }

        // now, iterate all ambiguity, determine the optimal receiver bias
        // for current epoch
        systemTypeBiasData.clear();
        for(auto stv: systemTypeVars)
        {
            typeVarSetMap typeVars = stv.second;
            for(auto tv: typeVars)
            {
                VariableSet vars = tv.second;
                VariableDataMap varNumFixed;

                int maxFixedNum(0);
                Variable varMaxFixed;

                cout << "corrected ambs:" << endl;
                for(auto v: vars)
                {
                    cout << floatAmbDataCorrected[v] << " : " ;
                }
                cout << endl;

                for(auto v: vars)
                {
                    cout << "datum amb:" << v << endl; 
                    double ambInt = std::floor(floatAmbDataCorrected[v] + 0.5);;
                    double recBias = floatAmbDataCorrected[v] - ambInt;
                    cout << "recBias:" << recBias << endl;

                    int numFixed(0);
                    for(auto v2: vars)
                    {
                        double ambFloat = floatAmbDataCorrected[v2] - recBias;
                        double ambInt = std::floor(ambFloat+0.5);

                        double bias = std::abs( ambFloat - ambInt);

                        if(bias<0.2)
                        {
                            numFixed++;
                        }
                    }

                    cout << "numFixed:" << numFixed << endl;

                    if(numFixed>maxFixedNum)
                    {
                        maxFixedNum = numFixed;
                        varMaxFixed = v;
                    }
                    cout << "varMaxFixed:" << varMaxFixed << endl;
                }

                // compute the mean recBias with fixed ambiguities
                double ambInt = std::floor(floatAmbDataCorrected[varMaxFixed] + 0.5);;
                double initialRecBias = floatAmbDataCorrected[varMaxFixed] - ambInt;
                double meanBias(0.0);
                int numFixed(0);
                for(auto v: vars)
                {
                    double ambFloat = floatAmbDataCorrected[v] - initialRecBias;
                    double ambInt = std::floor(ambFloat+0.5);
                    double bias = std::abs(ambFloat - ambInt);

                    if(bias<0.2)
                    {
                        double recBias = floatAmbDataCorrected[v] - ambInt;
                        meanBias += recBias;
                        numFixed++;
                    }
                }
                meanBias = meanBias/numFixed;
                systemTypeBiasData[stv.first][tv.first] = meanBias;

                cout << "meanBias" << meanBias << endl;
            }
        }

        //* receiver bias Connect
        if(firstTime)
        {
            firstTime = false;
        }
        else
        {
            systemTypeValueMap tempSystemTypeBias;
            for(auto stb: systemTypeBiasData)
            {
                typeValueMap typeBiasData = stb.second;
                for(auto tv: typeBiasData)
                {
                    double bias = tv.second;
                    double oldBias = oldSystemTypeBiasData[stb.first][tv.first];
                    double offset = std::floor( (bias - oldBias) + 0.5);
                    cout << "bias:" << bias << "oldBias:" << oldBias << "offset:" << offset << endl;
                    bias -= offset;
                    cout << "new bias:" << bias<< endl;
                    tempSystemTypeBias[stb.first][tv.first] = bias;
                }
            }
            // update 
            systemTypeBiasData = tempSystemTypeBias;
        }

        // store current bias data
        oldSystemTypeBiasData = systemTypeBiasData;

        // step 3: compute float ambiguities
        cout << "float Int:" << endl;
        for(auto amb: ambSet)
        {
            SatID sat = amb.getSatellite();
            SatelliteSystem::Systems system = sat.getSatelliteSystem();
            TypeID type = amb.getType();
            double recBias = systemTypeBiasData[system][type]; 
            double ambInt = floatAmbDataCorrected[amb] - recBias;

            cout << amb << ":" << ambInt << endl;
            floatIntAmbData[amb] = ambInt;
        }
    }


    void SolverAmb::fixWithRound(VariableDataMap& ambData,
                                 CovarianceDataMap& ambCovData, 
                                 VariableDataMap& fixedAmbData)
    {
        VariableDataMap ambDataFixed;
        for( auto amb : ambData )
        {
            double ambFloat;
            ambFloat = amb.second;

            double ambVar;
            ambVar = ambCovData[amb.first][amb.first];

              // ambiguity fixing
            ARData arData;
            arData = ambRound.resolve(ambFloat, ambVar);
            if(arData.fixed)
            {
                ambDataFixed[amb.first] = arData.ambFixed;
            }
        }
        fixedAmbData = ambDataFixed;
    }


    void SolverAmb::fixWithPAR(VariableDataMap& ambData, 
                               CovarianceDataMap& ambCovData,
                               VariableDataMap& fixedAmbData)
    {

        VariableDataMap tempAmbData = ambData;
        CovarianceDataMap tempAmbCovData = ambCovData;
        VariableDataMap tempFixedAmbData;

        // now, par
        double ratio(0.0);
        while(ratio < 3.0)
        {
            // exception
            if(tempAmbData.size() <= 3)
            {
                break;
            }

            // fix with lambda
            ratio = fixWithLambda(tempAmbData, tempAmbCovData, tempFixedAmbData);

            cout << "ratio:" << ratio << endl;

            if(ratio >= 2.0)
            {
                break;
            }
            else
            {
                double minPrec(9.9E+10);
                Variable ambMinPrec;
                for(auto amb: tempAmbData)
                {
                    double ambFloat = tempAmbData[amb.first];
                    double ambVar = tempAmbCovData[amb.first][amb.first];
                    ARData arData = ambRound.resolve(ambFloat, ambVar);

                    cout << amb.first << " prec:" << arData.precision << endl;
                    if( arData.precision < minPrec )
                    {
                        minPrec = arData.precision;
                        ambMinPrec = amb.first;
                    }
                }

                cout << "minPrec:" << ambMinPrec << endl;

                // erase the ambiguity with max-variance
                tempAmbData.erase(ambMinPrec);
            }
        }

        if(ratio>2.0)
        {
            fixedAmbData = tempFixedAmbData;
        }

    }

    double SolverAmb::fixWithLambda(VariableDataMap& ambData, 
                                    CovarianceDataMap& ambCovData,
                                    VariableDataMap& fixedAmbData)
    {
        // define ambVec/ambCovMatrix
        double namb = ambData.size();
        Vector<double> ambVec(namb, 0.0);
        Matrix<double> covMatrix(namb, namb, 0.0);

        // convert ambData/ambCovData to ambVec/covMatrix 
        int i = 0;
        for(auto amb : ambData)
        {
            ambVec(i) = amb.second;
            int j = 0;
            for(auto amb2 : ambData)
            {
                covMatrix(i,j) = ambCovData[amb.first][amb2.first];
                j++;
            }
            i++;
        }

        cout << "ambVec" << endl;
        cout << ambVec << endl;

        cout << "covMatrix" << endl;
        cout << covMatrix << endl;


        // resolve the ambiguties using MLAMBDA method
        ambMLambda.resolve(ambVec, covMatrix);

        Vector<double> fixedAmbVec(namb, 0.0);
        fixedAmbVec = ambMLambda.getFixedAmbVec();

        cout << "fixedAmbVec" << endl;
        cout << fixedAmbVec << endl;

        // now, save fixedAmbVec into allIntAmbDataFixed;
        i = 0;
        VariableDataMap tempFixedAmbData;
        for(auto amb: ambData)
        {
            tempFixedAmbData[amb.first] = fixedAmbVec[i];
            i++;
        }

        double ratio;
        ratio= ambMLambda.getRatio();

        fixedAmbData = tempFixedAmbData;

        return ratio;

    }

    void SolverAmb::computeFixedFloatAmbs(VariableDataMap& fixedIntAmbData, 
                                          VariableDataMap& fixedFloatAmbData)
    {
        fixedFloatAmbData.clear();
        for(auto amb:fixedIntAmbData)
        {
            SatID sat = amb.first.getSatellite();
            SatelliteSystem::Systems system = sat.getSatelliteSystem();
            TypeID type = amb.first.getType();

            double satBias = satTypeBiasData[sat][type];
            double recBias = systemTypeBiasData[system][type];
            double ambInt = amb.second;

            double floatFixedAmb = recBias + satBias + ambInt;
            fixedFloatAmbData[amb.first] = floatFixedAmb;
        }
    };

}  // End of namespace gpstk
